#include <iostream>
#include "Driver/HKMutiCam.hpp"

using namespace std;
using namespace cv;

pthread_mutex_t HKMutiCam::mutex_ {};
std::array<cv::Mat, CAMERA_NUM> HKMutiCam::frames_ {};
std::array<bool, CAMERA_NUM> HKMutiCam::frame_emptys_ {};

HKMutiCam::HKMutiCam()
{
    XInitThreads();

    for(int i = 0; i < CAMERA_NUM; i++)
    {
        handles.push_back(NULL);
        nThreadIDs.push_back(pthread_t());
    }
    camera_param = Camera_Param();
    save_count = 0;

    //********** 枚举设备 ********************************/
    MV_CC_DEVICE_INFO_LIST stDeviceList;
    memset(&stDeviceList, 0, sizeof(MV_CC_DEVICE_INFO_LIST));
    nRet = MV_CC_EnumDevices(MV_GIGE_DEVICE | MV_USB_DEVICE, &stDeviceList);
    if (MV_OK != nRet)
    {
        printf("MV_CC_EnumDevices fail! nRet [%x]\n", nRet);
        exit(-1);
    }
    unsigned int nIndex = 0;
    if (stDeviceList.nDeviceNum > 0)
    {
        for (int i = 0; i < CAMERA_NUM; i++)
        {
            printf("[device %d]:\n", i);
            MV_CC_DEVICE_INFO *pDeviceInfo = stDeviceList.pDeviceInfo[i];
            if (NULL == pDeviceInfo)
            {
                printf("Find Not Enough Devices!\n");
                exit(-1);
            }
            PrintDeviceInfo(pDeviceInfo);
        }
    }
    else
    {
        printf("Find No Devices!\n");
        exit(-1);
    }

    //初始化互斥量
    nRet = pthread_mutex_init(&mutex_, NULL);
    if (nRet != 0)
    {
        perror("pthread_create failed\n");
        exit(-1);
    }

    for(int i = 0; i < CAMERA_NUM; i++)
    {
        //********** 选择设备并创建句柄 *************************/

        nRet = MV_CC_CreateHandle(&handles[i], stDeviceList.pDeviceInfo[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_CreateHandle fail! nRet [%x]\n", nRet);
            exit(-1);
        }

        // 打开设备
        //********** frame **********/

        nRet = MV_CC_OpenDevice(handles[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_OpenDevice fail! nRet [%x]\n", nRet);
            exit(-1);
        }

        //设置默认的配置
        this->set(i, CAP_PROP_FRAMERATE_ENABLE, camera_param.FrameRateEnable);
        this->set(i, CAP_PROP_FRAMERATE, camera_param.FrameRate);
        // this->set(i, CAP_PROP_BURSTFRAMECOUNT, camera_param.BurstFrameCount);
        this->set(i, CAP_PROP_HEIGHT, camera_param.height);
        this->set(i, CAP_PROP_WIDTH, camera_param.width);
        this->set(i, CAP_PROP_OFFSETX, camera_param.Offset_x);
        this->set(i, CAP_PROP_OFFSETY, camera_param.Offset_y);
        this->set(i, CAP_PROP_EXPOSURE_TIME, camera_param.ExposureTime);
        this->set(i, CAP_PROP_GAMMA_ENABLE, camera_param.GammaEnable);
        this->set(i, CAP_PROP_GAMMA, camera_param.Gamma);
        this->set(i, CAP_PROP_GAINAUTO, camera_param.GainAuto);
        this->set(i, CAP_PROP_GAIN, camera_param.Gain);
        // this->set(CAP_PROP_TRIGGER_MODE, camera_param.TriggerMode);
        // this->set(CAP_PROP_TRIGGER_SOURCE, camera_param.TriggerSource);
        // this->set(CAP_PROP_LINE_SELECTOR, camera_param.LineSelector);

        //********** frame **********/
        //白平衡 自适应（给定参数1）

        nRet = MV_CC_SetEnumValue(handles[i], "BalanceWhiteAuto", 2);

    //    //白平衡度
    //    int rgb[3] = {1290, 1024, 1474};
    //    for (int i = 0; i < 3; i++)
    //    {
    //        //********** frame **********/

    //        nRet = MV_CC_SetEnumValue(handle, "BalanceRatioSelector", i);
    //        nRet = MV_CC_SetIntValue(handle, "BalanceRatio", rgb[i]);
    //    }
        if (MV_OK == nRet)
        {
            printf("set BalanceRatio OK!\n");
        }
        else
        {
            printf("Set BalanceRatio Failed! nRet = [%x]\n\n", nRet);
        }
        this->set(i, CAP_PROP_SATURATION_ENABLE, camera_param.SaturationEnable);
        this->set(i, CAP_PROP_SATURATION, camera_param.Saturation);
        //软件触发
        // ********** frame **********/
        nRet = MV_CC_SetEnumValue(handles[i], "TriggerMode", 0);
        if (MV_OK == nRet)
        {
            printf("set TriggerMode OK!\n");
        }
        else
        {
            printf("MV_CC_SetTriggerMode fail! nRet [%x]\n", nRet);
        }

        //********** 图像格式 **********/
        // 0x01100003:Mono10
        // 0x010C0004:Mono10Packed
        // 0x01100005:Mono12
        // 0x010C0006:Mono12Packed
        // 0x01100007:Mono16
        // 0x02180014:RGB8Packed
        // 0x02100032:YUV422_8
        // 0x0210001F:YUV422_8_UYVY
        // 0x01080008:BayerGR8
        // 0x01080009:BayerRG8
        // 0x0108000A:BayerGB8
        // 0x0108000B:BayerBG8
        // 0x0110000e:BayerGB10
        // 0x01100012:BayerGB12
        // 0x010C002C:BayerGB12Packed
        nRet = MV_CC_SetEnumValue(handles[i], "PixelFormat", 0x02180014); // 目前 RGB

        if (MV_OK == nRet)
        {
            printf("set PixelFormat OK!\n");
        }
        else
        {
            printf("MV_CC_SetPixelFormat fail! nRet [%x]\n", nRet);
        }
        MVCC_ENUMVALUE t = {0};
        //********** frame **********/

        nRet = MV_CC_GetEnumValue(handles[i], "PixelFormat", &t);

        if (MV_OK == nRet)
        {
            printf("PixelFormat :%d!\n", t.nCurValue); // 35127316
        }
        else
        {
            printf("get PixelFormat fail! nRet [%x]\n", nRet);
        }
        // 开始取流
        //********** frame **********/

        nRet = MV_CC_StartGrabbing(handles[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_StartGrabbing fail! nRet [%x]\n", nRet);
            exit(-1);
        }

        //********** frame **********/

        nRet = pthread_create(&nThreadIDs[i], NULL, HKWorkThread, handles[i]);

        if (nRet != 0)
        {
            printf("thread create failed.ret = %d\n", nRet);
            exit(-1);
        }
        else
        {
            printf("thread create succeeded.ret = %d\n", nRet);
        }
    }
}

HKMutiCam::~HKMutiCam()
{
    int nRet;
    //********** frame **********/

    for(int i = 0; i < CAMERA_NUM; i++)
    {
        pthread_join(nThreadIDs[i], NULL);

        //********** frame **********/

        nRet = MV_CC_StopGrabbing(handles[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_StopGrabbing fail! nRet [%x]\n", nRet);
            exit(-1);
        }
        printf("MV_CC_StopGrabbing succeed.\n");
        // 关闭设备
        //********** frame **********/

        nRet = MV_CC_CloseDevice(handles[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_CloseDevice fail! nRet [%x]\n", nRet);
            exit(-1);
        }
        printf("MV_CC_CloseDevice succeed.\n");
        // 销毁句柄
        //********** frame **********/

        nRet = MV_CC_DestroyHandle(handles[i]);

        if (MV_OK != nRet)
        {
            printf("MV_CC_DestroyHandle fail! nRet [%x]\n", nRet);
            exit(-1);
        }
        printf("MV_CC_DestroyHandle succeed.\n");
        // 销毁互斥量
        pthread_mutex_destroy(&mutex_);
    }

}

bool HKMutiCam::set(int id, HKMutiCam::CamerProperties type, float value)
{
    switch (type)
    {
    case CAP_PROP_FRAMERATE_ENABLE:
    {
        //********** frame **********/

        nRet = MV_CC_SetBoolValue(handles[id], "AcquisitionFrameRateEnable", value);

        if (MV_OK == nRet)
        {
            printf("set AcquisitionFrameRateEnable OK!\n");
        }
        else
        {
            printf("Set AcquisitionFrameRateEnable Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_FRAMERATE:
    {
        //********** frame **********/

        nRet = MV_CC_SetFloatValue(handles[id], "AcquisitionFrameRate", value);

        if (MV_OK == nRet)
        {
            printf("set AcquisitionFrameRate OK!\n");
        }
        else
        {
            printf("Set AcquisitionFrameRate Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_BURSTFRAMECOUNT:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "AcquisitionBurstFrameCount", value);

        if (MV_OK == nRet)
        {
            printf("set AcquisitionBurstFrameCount OK!\n");
        }
        else
        {
            printf("Set AcquisitionBurstFrameCount Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_HEIGHT:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "Height", value); //图像高度

        if (MV_OK == nRet)
        {
            printf("set Height OK!\n");
        }
        else
        {
            printf("Set Height Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_WIDTH:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "Width", value); //图像宽度

        if (MV_OK == nRet)
        {
            printf("set Width OK!\n");
        }
        else
        {
            printf("Set Width Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_OFFSETX:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "OffsetX", value); //图像宽度

        if (MV_OK == nRet)
        {
            printf("set Offset X OK!\n");
        }
        else
        {
            printf("Set Offset X Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_OFFSETY:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "OffsetY", value); //图像宽度

        if (MV_OK == nRet)
        {
            printf("set Offset Y OK!\n");
        }
        else
        {
            printf("Set Offset Y Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_EXPOSURE_TIME:
    {
        //********** frame **********/

        nRet = MV_CC_SetFloatValue(handles[id], "ExposureTime", value); //曝光时间

        if (MV_OK == nRet)
        {
            printf("set ExposureTime OK!\n");
        }
        else
        {
            printf("Set ExposureTime Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_GAMMA_ENABLE:
    {
        //********** frame **********/

        nRet = MV_CC_SetBoolValue(handles[id], "GammaEnable", value); //伽马因子是否可调  默认不可调（false）

        if (MV_OK == nRet)
        {
            printf("set GammaEnable OK!\n");
        }
        else
        {
            printf("Set GammaEnable Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_GAMMA:
    {
        //********** frame **********/

        nRet = MV_CC_SetFloatValue(handles[id], "Gamma", value); //伽马越小 亮度越大

        if (MV_OK == nRet)
        {
            printf("set Gamma OK!\n");
        }
        else
        {
            printf("Set Gamma Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_GAINAUTO:
    {
        //********** frame **********/

        nRet = MV_CC_SetEnumValue(handles[id], "GainAuto", value);

        if (MV_OK == nRet)
        {
            printf("set GainAuto OK!\n");
        }
        else
        {
            printf("Set GainAuto Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_GAIN:
    {
        //********** frame **********/

        nRet = MV_CC_SetFloatValue(handles[id], "Gain", value); //亮度 越大越亮

        if (MV_OK == nRet)
        {
            printf("set Gain OK!\n");
        }
        else
        {
            printf("Set Gain Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_SATURATION_ENABLE:
    {
        //********** frame **********/

        nRet = MV_CC_SetBoolValue(handles[id], "SaturationEnable", value); //饱和度是否可调 默认不可调(false)

        if (MV_OK == nRet)
        {
            printf("set SaturationEnable OK!\n");
        }
        else
        {
            printf("Set SaturationEnable Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_SATURATION:
    {
        //********** frame **********/

        nRet = MV_CC_SetIntValue(handles[id], "Saturation", value); //饱和度 默认128 最大255

        if (MV_OK == nRet)
        {
            printf("set Saturation OK!\n");
        }
        else
        {
            printf("Set Saturation Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }

    case CAP_PROP_TRIGGER_MODE:
    {

        nRet = MV_CC_SetEnumValue(handles[id], "TriggerMode", value); //饱和度 默认128 最大255

        if (MV_OK == nRet)
        {
            printf("set TriggerMode OK!\n");
        }
        else
        {
            printf("Set TriggerMode Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_TRIGGER_SOURCE:
    {

        nRet = MV_CC_SetEnumValue(handles[id], "TriggerSource", value); //饱和度 默认128 最大255255

        if (MV_OK == nRet)
        {
            printf("set TriggerSource OK!\n");
        }
        else
        {
            printf("Set TriggerSource Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }
    case CAP_PROP_LINE_SELECTOR:
    {

        nRet = MV_CC_SetEnumValue(handles[id], "LineSelector", value); //饱和度 默认128 最大255

        if (MV_OK == nRet)
        {
            printf("set LineSelector OK!\n");
        }
        else
        {
            printf("Set LineSelector Failed! nRet = [%x]\n\n", nRet);
        }
        break;
    }

    default:
        return 0;
    }
    return nRet;
}

bool HKMutiCam::reset(int id)
{
    nRet = this->set(id, CAP_PROP_FRAMERATE_ENABLE, camera_param.FrameRateEnable);
    nRet = this->set(id, CAP_PROP_FRAMERATE, camera_param.FrameRate) || nRet;
    // nRet = this->set(id, CAP_PROP_BURSTFRAMECOUNT, camera_param.BurstFrameCount) || nRet;
    nRet = this->set(id, CAP_PROP_HEIGHT, camera_param.height) || nRet;
    nRet = this->set(id, CAP_PROP_WIDTH, camera_param.width) || nRet;
    nRet = this->set(id, CAP_PROP_OFFSETX, camera_param.Offset_x) || nRet;
    nRet = this->set(id, CAP_PROP_OFFSETY, camera_param.Offset_y) || nRet;
    nRet = this->set(id, CAP_PROP_EXPOSURE_TIME, camera_param.ExposureTime) || nRet;
    nRet = this->set(id, CAP_PROP_GAMMA_ENABLE, camera_param.GammaEnable) || nRet;
    nRet = this->set(id, CAP_PROP_GAMMA, camera_param.Gamma) || nRet;
    nRet = this->set(id, CAP_PROP_GAINAUTO, camera_param.GainAuto) || nRet;
    nRet = this->set(id, CAP_PROP_GAIN, camera_param.Gain) || nRet;
    nRet = this->set(id, CAP_PROP_SATURATION_ENABLE, camera_param.SaturationEnable) || nRet;
    nRet = this->set(id, CAP_PROP_SATURATION, camera_param.Saturation) || nRet;
    nRet = this->set(id, CAP_PROP_TRIGGER_MODE, camera_param.TriggerMode) || nRet;
    nRet = this->set(id, CAP_PROP_TRIGGER_SOURCE, camera_param.TriggerSource) || nRet;
    nRet = this->set(id, CAP_PROP_LINE_SELECTOR, camera_param.LineSelector) || nRet;
    return nRet;
}

bool HKMutiCam::PrintDeviceInfo(MV_CC_DEVICE_INFO *pstMVDevInfo)
{
    if (NULL == pstMVDevInfo)
    {
        printf("%s\n", "The Pointer of pstMVDevInfoList is NULL!");
        return false;
    }
    if (pstMVDevInfo->nTLayerType == MV_GIGE_DEVICE)
    {
        printf("%s %x\n", "nCurrentIp:", pstMVDevInfo->SpecialInfo.stGigEInfo.nCurrentIp);                 //当前IP
        printf("%s %s\n\n", "chUserDefinedName:", pstMVDevInfo->SpecialInfo.stGigEInfo.chUserDefinedName); //用户定义名
    }
    else if (pstMVDevInfo->nTLayerType == MV_USB_DEVICE)
    {
        printf("UserDefinedName:%s\n\n", pstMVDevInfo->SpecialInfo.stUsb3VInfo.chUserDefinedName);
    }
    else
    {
        printf("Not support.\n");
    }
    return true;
}

void HKMutiCam::ReadImg(vector<cv::Mat> &images)
{
    images.clear();
    pthread_mutex_lock(&mutex_);
    for(int i = 0; i < CAMERA_NUM; i++)
    {
        images.push_back(frames_[i].clone());
    }
    pthread_mutex_unlock(&mutex_);
    waitKey(5);
}

void HKMutiCam::saveImg()
{
    pthread_mutex_lock(&mutex_);
//    for(int i = 0; i < CAMERA_NUM; i++)
//    {
//        if (frame_emptys_[i])
//        {
//            return;
//        }
//    }
    for(int i = 0; i < CAMERA_NUM; i++)
    {
        imwrite("/home/ssj5vkjcty/HKCamera/"+to_string(i)+"/"+to_string(save_count)+"_"+to_string(i)+".bmp",frames_[i]);
    }
    save_count++;
    cout<<save_count<<endl;
    pthread_mutex_unlock(&mutex_);
}

void* HKMutiCam::HKWorkThread(void* p_handle)
{
    double start;
    int nRet;
    unsigned char *m_pBufForDriver = (unsigned char *)malloc(sizeof(unsigned char) * MAX_IMAGE_DATA_SIZE);
    unsigned char *m_pBufForSaveImage = (unsigned char *)malloc(MAX_IMAGE_DATA_SIZE);
    MV_FRAME_OUT_INFO_EX stImageInfo = {0};
    MV_CC_PIXEL_CONVERT_PARAM stConvertParam = {0};

    int image_empty_count = 0; //空图帧数
    while (1)
    {
        start = static_cast<double>(cv::getTickCount());
        MVCC_STRINGVALUE stStringValue = {0};
        nRet = MV_CC_GetStringValue(p_handle, "DeviceUserID", &stStringValue);
        if (MV_OK == nRet)
        {
            //printf("Get DeviceUserID [%s]\n\n", stStringValue.chCurValue);
        }
        else
        {
            printf("Get DeviceUserID Failed! nRet = [%x]\n\n", nRet);
        }
        int id = atoi(stStringValue.chCurValue);

        nRet = MV_CC_GetOneFrameTimeout(p_handle, m_pBufForDriver, MAX_IMAGE_DATA_SIZE, &stImageInfo, 25);
        if (nRet != MV_OK)
        {
            //cout<<hex<<nRet<<" "<<id<<endl;
            if (++image_empty_count > 100)
            {
                cout<<id<<"The Number of Faild Reading Exceed The Set Value!\n";
                exit(-1);
            }
            continue;
        }
        image_empty_count = 0; //空图帧数
        //转换图像格式为BGR8

        stConvertParam.nWidth = 1280;                               //ch:图像宽 | en:image width
        stConvertParam.nHeight = 1024;                              //ch:图像高 | en:image height
        stConvertParam.pSrcData = m_pBufForDriver;                  //ch:输入数据缓存 | en:input data buffer
        stConvertParam.nSrcDataLen = MAX_IMAGE_DATA_SIZE;           //ch:输入数据大小 | en:input data size
        stConvertParam.enDstPixelType = PixelType_Gvsp_BGR8_Packed; //ch:输出像素格式 | en:output pixel format                      //! 输出格式 RGB
        stConvertParam.pDstBuffer = m_pBufForSaveImage;             //ch:输出数据缓存 | en:output data buffer
        stConvertParam.nDstBufferSize = MAX_IMAGE_DATA_SIZE;        //ch:输出缓存大小 | en:output buffer size
        stConvertParam.enSrcPixelType = stImageInfo.enPixelType;    //ch:输入像素格式 | en:input pixel format                       //! 输入格式 RGB
        MV_CC_ConvertPixelType(p_handle, &stConvertParam);


        pthread_mutex_lock(&mutex_);
        frames_[id] = cv::Mat(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC3, m_pBufForSaveImage).clone(); //tmp.clone();
        frame_emptys_[id] = 0;
        double time = ((double)cv::getTickCount() - start) / cv::getTickFrequency();
        //*************************************testing img********************************//
//        std::cout << "HK_camera,Time:" <<id<<"  "<< time << "\tFPS:" << 1 / time << std::endl;
//        imshow("HK vision"+to_string(id), cv::Mat(stImageInfo.nHeight, stImageInfo.nWidth, CV_8UC3, m_pBufForSaveImage));
//        waitKey(1);
        pthread_mutex_unlock(&mutex_);

    }
    free(m_pBufForDriver);
    free(m_pBufForSaveImage);
    return 0;
}
